1. Field of the Invention
The present invention relates to a technique for measuring the position and orientation of an object whose three-dimensional shape is given and, more particularly, to a position and orientation measurement apparatus which calculates the position and orientation of a target object based on a range image obtained by capturing an image of the target object, a position and orientation measurement method, and a storage medium.
2. Description of the Related Art
Along with the development of robot technologies in recent years, robots perform complicated tasks, which were manually performed conventionally. As a representative example of such complicated tasks, assembling processes of industrial products are known. Such robot has to hold parts by an end effecter such as a hand, so as to autonomously perform assembling processes. In order to control the robot to hold a part by a robot hand, a relative position and orientation between the part to be held and the robot have to be measured. Then, a moving plan must be designed based on the measurement result so as to actually control actuators. The position and orientation of the robot are measured using a camera or distance sensor mounted on the robot, and methods using a two-dimensional image and range image are typical. Especially, in a scene of an office or factory, when a target such as a part of an industrial product has only monotonic features such as lines, curves, and planes with small density changes, a range image is normally used.
P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239-256, 1992. discloses a method to measure the position and orientation of an object by model fitting to a range image. With this method, a range image is converted into a three-dimensional point cloud data, and a three-dimensional shape model of a target object is fitted to the point cloud data, thereby measuring the position and orientation of the object. That is, for respective points of the point cloud data, adjacent planes of the three-dimensional shape model are searched for based on coarse values of the position and orientation, and optimization of the position and orientation is repeated to minimize a sum of distances between points and planes, thereby calculating the position and orientation.
U.S. Pat. No. 6,504,957 discloses a method of estimating the position and orientation of an object by model fitting to an observation image as in P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239-256, 1992. At this time, a small-curvature plane is selected from a model of the object, and a difference of a distance between the selected plane and a corresponding plane on distance measurement data is minimized.
The method of estimating the position and orientation of an object using a range image as in P. J. Besl and N. D. McKay, “A method for registration of 3-D shapes,” IEEE Transactions on Pattern Analysis and Machine Intelligence, vol. 14, no. 2, pp. 239-256, 1992. suffers a problem that requires a long calculation time for search processing of planes of the three-dimensional model corresponding to respective points of the point cloud data of the range image.
As a solution of this problem, a method of reducing the number of correspondences may be used. However, measurement data always include errors. When the number of correspondences is reduced, a degree of contribution of each individual corresponding point to calculation processing upon estimation of the position and orientation becomes relatively high. Therefore, when the corresponding points include those based on measurement data having large errors, the position/orientation estimation precision lowers.
To solve the aforementioned problem, U.S. Pat. No. 6,504,957 assumes that small-curvature portions on a measurement target object suffer small distance measurement errors, and limits a processing target to these small-curvature portions on the model, thereby speeding up the processing without lowering the precision.
However, in practice, even a small-curvature portion does not always have high distance measurement precision. This will be described below with reference to FIG. 1. FIG. 1 is a principle view for explaining the aforementioned phenomenon. Referring to FIG. 1, an image capturing unit 101 configures a distance measurement apparatus (to be also referred to as a range image measurement apparatus hereinafter). A measurement target object 102 has portions having small curvatures. Points 103 and 104 as two points on the surface of the measurement target object 102 will be examined. In this case, the point 104 has a smaller curvature than that of the point 103. However, upon examination of regions 105 and 106 that observe pixels corresponding to regions including the points 103 and 104, a distribution of distances from the image capturing unit 101 of the surface on the measurement target object included in the region 106 extends over a broader range than the region 105. Therefore, a variation error of a distance measurement value of the point 104 is larger than that of a distance measurement value of the point 103. In this manner, a small-curvature portion often has lower distance measurement precision than a large-curvature portion depending on an inclination of a plane. In such case, according to the method of U.S. Pat. No. 6,504,957, since the position and orientation of an object are estimated by selecting a region having large variation errors of distance measurement values (a small-curvature region), the position/orientation estimation precision becomes lower than that obtained when the method of U.S. Pat. No. 6,504,957 is not used.
That is, with the method of U.S. Pat. No. 6,504,957, since data with smaller measurement errors cannot be reliably selected, the position/orientation estimation precision is lowered unwantendly.
In consideration of the aforementioned problems, the present invention provides a technique for maintaining and improving a processing speed while preventing the position/orientation estimation precision from being lowered.